function [ trajecS ] = generateTrajSelfLoc( p_nd, varV, N, destin )
[n_nd, ~] = size(p_nd);
trajecS = zeros(4,N,n_nd);

A = diag(ones(4,1));
A(1,3) = 1;
A(2,4) = 1;

W = zeros(4,2);
W(1,1) = 0.5;
W(2,2) = 0.5;
W(3,1) = 1;
W(4,2) = 1;

for i = 1:n_nd
    distto = destin(i,:) - p_nd(i,:);
    x = [p_nd(i,1); p_nd(i,2); distto(1)/N; distto(2)/N];
    
    for n=1:N
        trajecS(:,n,i) = x;
        x = A*x + W*[sqrt(varV)*randn; sqrt(varV)*randn];
    end
end

end

